﻿#include "calibration/calibration.h"

Calibration::Calibration(CalibrationConfig *config, VisionLocation *location, QObject *parent)
    : QObject(parent), config(config), location(location)
{
    if (config->hasCalibration())
    {
        auto cameraConfig = location->camera()->config();
        QPointF image_center(cameraConfig->imageWidth() / 2, cameraConfig->imageHeight() / 2);
        mapping.ChangeParameter(QMatrix(config->matrix11(), config->matrix12(), config->matrix21(), config->matrix22(),
                                        config->deltaX(), config->deltaY()),
                                image_center);
        mOriginB = mA2BMapping.pixel2MechPoint(image_center);
        mRotationB = caculateRotationAngle();
    }
}

QString Calibration::calibrationName() const
{
    return config->calibrationName();
}

void Calibration::performCalibration()
{
    qCInfo(visionCate()) << "Performing calibration" << calibrationName();

    config->setHasCalibration(false);
    mapping.setHasCalibration(false);

    double xMove = config->calibrationStep();
    double yMove = config->calibrationStep();
    QVector<QPointF> pixelPoints;
    QVector<QPointF> motorPoints;
    location->openLight();

    if (config->useCustomMoveFunc())
    {
        if (customCaliFunc == nullptr)
        {
            throw SilicolAbort(tr("Custom calibration function was not set!"), EX_LOCATION);
        }
        else
        {
            customCaliFunc(xMove, yMove, pixelPoints, motorPoints);
        }
    }
    else
    {
        getCorrespondingPixelMotorPoints(xMove, yMove, pixelPoints, motorPoints);
    }

    double thetaToX
        = qAtan((pixelPoints[0].y() - pixelPoints[1].y()) / (pixelPoints[0].x() - pixelPoints[1].x())) * 180 / M_PI;
    double thetaToY
        = qAtan((pixelPoints[2].x() - pixelPoints[3].x()) / (pixelPoints[2].y() - pixelPoints[3].y())) * 180 / M_PI;
    qCInfo(visionCate()) << calibrationName() << "CameraToMotorXAngle:" << thetaToX
                         << "CameraToMotorYAngle:" << thetaToY;

    coordinateA2BMapping(pixelPoints, motorPoints);

    config->setOriginX(mOriginB.x());
    config->setOriginY(mOriginB.y());
    config->setMatrix11(mA2BMatrix.m11());
    config->setMatrix12(mA2BMatrix.m12());
    config->setMatrix21(mA2BMatrix.m21());
    config->setMatrix22(mA2BMatrix.m22());
    config->setDeltaX(mA2BMatrix.dx());
    config->setDeltaY(mA2BMatrix.dy());
    config->setHasCalibration(true);

    auto cameraConfig = location->camera()->config();
    QPointF image_center(cameraConfig->imageWidth() / 2, cameraConfig->imageHeight() / 2);
    mapping.ChangeParameter(QMatrix(config->matrix11(), config->matrix12(), config->matrix21(), config->matrix22(),
                                    config->deltaX(), config->deltaY()),
                            image_center);
}

void Calibration::performPRResult(PrOffset offset)
{
    if (config->useCustomMoveFunc())
    {
        if (customPerformPrResultFunc == nullptr)
        {
            throw SilicolAbort(tr("Custom perform pr result function was not set!"), EX_LOCATION);
        }
        else
        {
            customPerformPrResultFunc(offset);
        }
    }
    else
    {
        QVariantList args1;
        args1 << offset.X * -1;
        MotionManager::getIns().executeInstruction(MotionElement::Axis, config->motorXName(), "relMove", args1);

        QVariantList args2;
        args2 << offset.Y * -1;
        MotionManager::getIns().executeInstruction(MotionElement::Axis, config->motorYName(), "relMove", args2);
    }
}

void Calibration::performPROriginResult(PrOffset offset)
{
    QVariantList args1;
    args1 << offset.O_X * -1;
    MotionManager::getIns().executeInstruction(MotionElement::Axis, config->motorXName(), "relMove", args1);

    QVariantList args2;
    args2 << offset.O_Y * -1;
    MotionManager::getIns().executeInstruction(MotionElement::Axis, config->motorYName(), "relMove", args2);
}

double Calibration::getRotationAngle()
{
    return mRotationB;
}

bool Calibration::getDeltaDistanceFromCenter(const QPointF pixelPoint, QPointF &distanceMech)
{
    if (mapping.hasCalibration())
    {
        mapping.CalcMechDistance(pixelPoint, distanceMech);
        qCInfo(visionCate()) << config->calibrationName() << "mech result:" << distanceMech.x() << distanceMech.y();
        return true;
    }
    return false;
}

QPointF Calibration::getOnePxielDistance()
{
    QPointF zero_zero, one_one;
    if (!getDeltaDistanceFromCenter(QPointF(0, 0), zero_zero))
        return QPointF();
    if (!getDeltaDistanceFromCenter(QPointF(1, 1), one_one))
        return QPointF();
    return QPointF(one_one.x() - zero_zero.x(), one_one.y() - zero_zero.y());
}

QPointF Calibration::getOneXPxielDistance()
{
    QPointF zero_zero, one_one;
    if (!getDeltaDistanceFromCenter(QPointF(0, 0), zero_zero))
        return QPointF();
    if (!getDeltaDistanceFromCenter(QPointF(1, 0), one_one))
        return QPointF();
    return QPointF(one_one.x() - zero_zero.x(), one_one.y() - zero_zero.y());
}

QPointF Calibration::getOneYPxielDistance()
{
    QPointF zero_zero, one_one;
    if (!getDeltaDistanceFromCenter(QPointF(0, 0), zero_zero))
        return QPointF();
    if (!getDeltaDistanceFromCenter(QPointF(0, 1), one_one))
        return QPointF();
    return QPointF(one_one.x() - zero_zero.x(), one_one.y() - zero_zero.y());
}

bool Calibration::getMechPoint(QPointF pixelPoint, QPointF &mechPoint)
{
    if (mapping.hasCalibration())
    {
        mechPoint = mapping.pixel2MechPoint(pixelPoint);
        return true;
    }
    return false;
}

bool Calibration::getCaliMapping(Pixel2Mech &caliMapping)
{
    if (mapping.hasCalibration())
    {
        auto cameraConfig = location->camera()->config();
        QPointF image_center(cameraConfig->imageWidth() / 2, cameraConfig->imageHeight() / 2);
        caliMapping.ChangeParameter(QMatrix(config->matrix11(), config->matrix12(), config->matrix21(),
                                            config->matrix22(), config->deltaX(), config->deltaY()),
                                    image_center);
        return true;
    }
    else
    {
        return false;
    }
}

Pixel2Mech *Calibration::getCaliMapping()
{
    return &mapping;
}

void Calibration::setCustomFunc(CustomCaliFunc customCaliFunc, CustomPerformPrResultFunc customPerformPrResultFunc)
{
    this->customCaliFunc = customCaliFunc;
    this->customPerformPrResultFunc = customPerformPrResultFunc;
}

void Calibration::coordinateA2BMapping(const QVector<QPointF> &APoints, const QVector<QPointF> &BPoints)
{
    auto cameraConfig = location->camera()->config();
    QPointF image_center(cameraConfig->imageWidth() / 2, cameraConfig->imageHeight() / 2);
    mA2BMatrix = mA2BMapping.DoCalibration(APoints.data(), BPoints.data(), image_center);
    QPointF A2BPoint = mA2BMapping.pixel2MechPoint(APoints[3]);
    QPointF diff = A2BPoint - BPoints[3];
    if (diff.x() * diff.x() + diff.y() * diff.y() > 0.01)
    {
        qCCritical(visionCate()) << config->calibrationName() << "Failed!";
    }
    else
    {
        qCInfo(visionCate()) << config->calibrationName() << "Successful!";
    }
    qCInfo(visionCate()) << "Valid b point, x, y:" << BPoints[3].x() << BPoints[3].y();
    qCInfo(visionCate()) << "A2B Point, x, y:" << A2BPoint.x() << A2BPoint.y();
    qCInfo(visionCate()) << "diff_x, diff_y:" << diff.x() << diff.y();

    QPointF A2BPoint0 = mA2BMapping.pixel2MechPoint(QPoint(0, 1));
    qCInfo(visionCate()) << "(0,1) A2BPoint0.x, A2BPoint0.y:" << A2BPoint0.x() << A2BPoint0.y();

    QPointF A2BPoint1 = mA2BMapping.pixel2MechPoint(QPoint(1, 0));
    qCInfo(visionCate()) << "(1,0) A2BPoint1.x, A2BPoint1.y:" << A2BPoint1.x() << A2BPoint1.y();

    mIsMapping = true;
    mOriginB = mA2BMapping.pixel2MechPoint(image_center);
    mRotationB = caculateRotationAngle();
    qCInfo(visionCate()) << config->calibrationName() << "Center x, y, rotation:" << mOriginB.x() << mOriginB.y()
                         << mRotationB;
}

double Calibration::caculateRotationAngle()
{
    double sin2cos2 = qSqrt(config->matrix11() * config->matrix11() + config->matrix12() * config->matrix12());
    double cos_a = qAcos(config->matrix11() / sin2cos2) * 180 / 3.14159265;
    double sin_b = qAsin(config->matrix12() / sin2cos2) * 180 / 3.14159265;
    // todo 判断象限
    return cos_a;
}

void Calibration::GetPixelPoint(double &x, double &y)
{
    PRResultStruct prResult;
    location->performPR(prResult);
    qCInfo(visionCate()) << config->calibrationName() << "PR result:" << prResult.toString();
    x = prResult.x;
    y = prResult.y;
}

void Calibration::getCorrespondingPixelMotorPoints(double xMove,
                                                   double yMove,
                                                   QVector<QPointF> &pixelPoints,
                                                   QVector<QPointF> &motorPoints)
{
    double pixel_x, pixel_y;
    for (int i = 1; i <= 4; i++)
    {
        if (i == 1)
        {
            QVariantList args;
            args << xMove;
            MotionManager::getIns().runInstruction(MotionElement::Axis, config->motorXName(), "relMove", args);
        }
        else if (i == 2)
        {
            QVariantList args;
            args << xMove * -2;
            MotionManager::getIns().runInstruction(MotionElement::Axis, config->motorXName(), "relMove", args);
        }
        else if (i == 3)
        {
            QVariantList args1;
            args1 << xMove;
            QString uuid1 = MotionManager::getIns().executeInstruction(MotionElement::Axis, config->motorXName(),
                                                                       "relMove", args1, true);
            QVariantList args2;
            args2 << yMove;
            QString uuid2 = MotionManager::getIns().executeInstruction(MotionElement::Axis, config->motorYName(),
                                                                       "relMove", args2, true);
            MotionManager::getIns().getInstructionExecutionResult(uuid1);
            MotionManager::getIns().getInstructionExecutionResult(uuid2);
        }
        else if (i == 4)
        {
            QVariantList args;
            args << yMove * -2;
            MotionManager::getIns().runInstruction(MotionElement::Axis, config->motorYName(), "relMove", args);
        }
        Sleep(300);
        GetPixelPoint(pixel_x, pixel_y);
        double xPos = MotionManager::getIns().getAxisCurrentPos(config->motorXName());
        double yPos = MotionManager::getIns().getAxisCurrentPos(config->motorYName());
        qCInfo(visionCate()) << config->calibrationName() << "Mech x, y: " << xPos << yPos;
        pixelPoints.append(QPointF(pixel_x, pixel_y));
        motorPoints.append(QPointF(xPos, yPos));
    }
}

bool Calibration::calculateMatrixAttribute(
    QVector<QPointF> p, QVector<QPointF> m, double &scaleX, double &scaleY, double &closestAngle)
{
    double diff_py10 = p[1].y() - p[0].y();
    double diff_px10 = p[1].x() - p[0].x();
    double diff_py30 = p[3].y() - p[0].y();
    double diff_px30 = p[3].x() - p[0].x();
    double diff_mx10 = m[1].x() - m[0].x();
    double diff_mx30 = m[3].x() - m[0].x();
    double diff_my10 = m[1].y() - m[0].y();
    double diff_my30 = m[3].y() - m[0].y();
    double a_11 = (diff_py10 * diff_mx30 - diff_py30 * diff_mx10) / (diff_px30 * diff_py10 - diff_py30 * diff_px10);
    double a_12 = (diff_mx10 - diff_px10 * a_11) / diff_py10;
    double a_13 = (m[0].x() - p[0].x() * a_11 - p[0].y() * a_12);

    double a_21 = (diff_py10 * diff_my30 - diff_py30 * diff_my10) / (diff_px30 * diff_py10 - diff_py30 * diff_px10);
    double a_22 = (diff_my10 - diff_px10 * a_21) / diff_py10;
    double a_23 = (m[0].y() - p[0].x() * a_21 - p[0].y() * a_22);

    double s_x = sqrt(a_11 * a_11 + a_21 * a_21);
    double s_y = sqrt(a_12 * a_12 + a_22 * a_22);
    if (s_x == 0 || s_y == 0)
    {
        return false;
    }
    scaleX = 1 / s_x;
    scaleY = 1 / s_y;
    double angle = acos(a_11 / s_x) * 180;
    double steppedAngle = angle - 360 * ((int)((int)angle / 360));
    closestAngle = 0;
    if (steppedAngle <= 45)
    {
        closestAngle = 0;
    }
    else if (steppedAngle <= 135)
    {
        closestAngle = 90;
    }
    else if (steppedAngle <= 225)
    {
        closestAngle = 180;
    }
    else if (steppedAngle <= 315)
    {
        closestAngle = 270;
    }
    return true;
}
